;;;;=========================================================================
;;;;=========================================================================
;;;;
;;;; This file provides various utility functions and data structures for
;;;; the search and rescue client.
;;;;
;;;; DO NOT MODIFY THE CONTENTS OF THIS FILE -- it may be overwritten in 
;;;; later releases of the Search and Rescue simulator.
;;;;
;;;; Authors: Eric Eaton, Aaron Curtis
;;;;
;;;;=========================================================================
;;;;=========================================================================




;;;;=========================================================================
;;;;
;;;; Data Structures and Associated Accessor Functions
;;;;
;;;;=========================================================================


;;;; Data Struct: coordinates
;;;; Describes a coordinate location in the gridworld
(defstruct coordinates
  ;; The x-coordinate
  (x nil :type real)
  ;; The y-coordinate
  (y nil :type real))

;;;; Shortcut generic accessor for the x-coordinate
(defmethod x-coordinate ((coord coordinates))
  (coordinates-x coord))

;;;; Shortcut generic accessor for the y-coordinate
(defmethod y-coordinate ((coord coordinates))
  (coordinates-y coord))



;;;; Data Struct: cell
;;;; Describes a cell in the gridworld
(defstruct cell
  ;; A coordinates struct representing the object's coordinates
  (coordinates nil :type coordinates)
  ;; A string representing the cell's marker
  (marker nil)
  ;; A list of the walls around the cells, represented
  ;; as strings of the cardinal directions ("north", "south", "east", "west")
  (walls nil)
  ;; The structure describing the occupant of the cells, which might
  ;; be an object or a superclass of the object data struct.
  (occupant nil))

;;;; Shortcut generic accessor for the coordinates
(defmethod coordinates ((obj cell))
  (cell-coordinates obj))




;;;; Data Struct: object
;;;; Describes an object in the gridworld
(defstruct object
  ;; The object's coordinates as a coordinates struct
  (coordinates nil :type coordinates)
  ;; A string representing the object's name
  (name nil :type string)
  ;; A list of feature describing the object
  (description))

;;;; Shortcut generic accessor for the name
(defmethod name ((obj object))
  (object-name obj))

;;;; Shortcut generic accessor for the coordinates
(defmethod coordinates ((obj object))
  (object-coordinates obj))

;;;; Shortcut generic accessor for the description
(defmethod description ((obj object))
  (object-description obj))




;;;; Data Struct: agent
;;;; Describes an agent in the gridworld
;;;; Inherits from object, so all object functions work on it
(defstruct (agent (:include object)))

;;;; Shortcut generic accessor for the name
(defmethod name ((obj agent))
  (agent-name obj))

;;;; Shortcut generic accessor for the coordinates
(defmethod coordinates ((obj agent))
  (agent-coordinates obj))

;;;; Shortcut generic accessor for the description
(defmethod description ((obj agent))
  (agent-description obj))



;;;; Data Struct: action
;;;; Describes an action
(defstruct action
  ;; a string representing the name of the action
  (name "NOOP" :type string)
  ;; a string representing the direction of the action, if any
  (direction nil))




;;;; Data Struct: action-status
;;;; Contains status information on the agent's last action
(defstruct action-status
  ;; the action itself
  (action nil :type action)
  ;; the timestamp of the action
  (timestamp nil :type real)
  ;; the status of the action. The format of this will vary
  ;; depending on the action.  See documentation for more details.
  (status))

;;;; Shortcut accessor function for the action
(defmethod action ((actstatus action-status))
  (action-status-action actstatus))

;;;; Shortcut accessor function for the timestamp
(defmethod timestamp ((actstatus action-status))
  (action-status-timestamp actstatus))

;;;; Shortcut accessor function for the status
(defmethod status ((actstatus action-status))
  (action-status-status actstatus))




;;;; Data Struct: perceptions
;;;; Contains all the perceptions of the agent for the current timestep.
(defstruct perceptions 
  ;; the action-status struct for the last action
  (last_action nil :read-only t)
  ;; the agent struct for itself
  (self_sensors nil :type agent :read-only t)
  ;; an agent or object struct, or nil if the short-range sensors weren't activated
  (shortrange_sensors nil :read-only t)
  ;; a list of cell structs 
  (longrange_sensors nil :read-only t))




;;;;==========================================================================
;;;;
;;;; Global parameters
;;;;
;;;;==========================================================================

;;;; A hashmap from a coordinate to a list of the coordinates adjacent to that coordinate on the map.
(defparameter *adjacent_coordinates* nil)

;;;; A hashmap from a marker to a list of the coordinates with the marker.
(defparameter *marked_coordinates* nil)

;;;; Stores the current simulation time
(defparameter *current_time* nil)





;;;;===========================================================================
;;;;
;;;; Map functions
;;;;
;;;;===========================================================================


;;;; Function:  get-adjacent-coordinates
;;;;
;;;; Retrieves a list of the coordinates adjacent to the given coordinate on the map.
;;;;
;;;; Arguments:
;;;;  coord - the coordinate
;;;;
;;;; Returns:
;;;;  a list of coordinates adjacent to the given coordinate.

(defmethod get-adjacent-coordinates ((coord coordinates))
    (gethash coord *adjacent_coordinates*))



;;;; Function:  adjacent-coordinates-p
;;;;
;;;; Determines whether two coordinates are adjacent on the map.
;;;;
;;;; Arguments:
;;;;  coord1 coord2 - the coordinates to test
;;;;
;;;; Returns:
;;;;  t if the coordinates are adjacent on the map, nil otherwise.

(defmethod adjacent-coordinates-p ((coords1 coordinates) (coords2 coordinates))
    (not (null (find-if
		#'(lambda (x) (equalp x coords2))
		(get-adjacent-coordinates coords1)))))



;;;; Function: add-adjacency
;;;;
;;;; Adds an adjacency between two coordinates.
;;;;
;;;; Arguments:
;;;;  coords1 - the first coordinates
;;;;  coords2 - the second coordinates
;;;; 
;;;; Returns:
;;;;  t if the adjacency was added, nil if it already existed.

(defmethod add-adjacency ((coords1 coordinates) (coords2 coordinates) &aux adjCoords (returnValue nil))

  ;; add the adjacency to coords1's adjacent coordinates
  (setf adjCoords (gethash coords1 *adjacent_coordinates*))
  (if (null (find coords2 adjCoords :test #'equalp))
      (progn 
	(setf (gethash coords1 *adjacent_coordinates*)
	      (append adjCoords (list coords2)))
	(setf returnValue t)))

  ;; add the adjacency to coords2's adjacent coordinates
  (setf adjCoords (gethash coords2 *adjacent_coordinates*))
  (if (null (find coords1 adjCoords :test #'equalp))
      (progn
	(setf (gethash coords2 *adjacent_coordinates*)
	      (append adjCoords (list coords1)))
	(setf returnValue t)))
  ;; return the returnValue
  returnValue)



;;;; Function: remove-adjacency
;;;;
;;;; Removes an adjacency between two coordinates.
;;;;
;;;; Arguments:
;;;;  coords1 - the first coordinates
;;;;  coords2 - the second coordinates
;;;; 

(defmethod remove-adjacency ((coords1 coordinates) (coords2 coordinates) &aux adjCoords)
  
  ;; removes the adjacency from coords1's adjacent coordinates
  (setf adjCoords (gethash coords1 *adjacent_coordinates*))
  (setf (gethash coords1 *adjacent_coordinates*)
	(remove coords2 adjCoords :test #'equalp))

  ;; removes the adjacency from coords2's adjacent coordinates
  (setf adjCoords (gethash coords2 *adjacent_coordinates*))
  (setf (gethash coords2 *adjacent_coordinates*)
	(remove coords1 adjCoords :test #'equalp))
  t)


    


;;;; Function:  get-marked-coordinates
;;;;
;;;; Retrieves a list of the coordinates with the given marker.
;;;;
;;;; Arguments:
;;;;  marker - the marker to match
;;;;
;;;; Returns:
;;;;  a list of coordinates with the given marker.

(defun get-marked-coordinates (marker)
    (gethash marker *marked_coordinates*))



;;;; Function:  has-marker-p
;;;;
;;;; Determines whether the given coordinate has the given marker.
;;;;
;;;; Arguments:
;;;;  coord - the coordinate to test
;;;;  marker - the marker
;;;;
;;;; Returns:
;;;;  t if the coordinate has the given marker, nil otherwise.

(defmethod has-marker-p ((coord coordinates) marker)
    (not (null (find-if
		#'(lambda (x) (equalp x coord))
		(get-marked-coordinates marker)))))



;;;; Function: add-marker
;;;;
;;;; Add a marker on the given coordinates.
;;;;
;;;; Arguments:
;;;;  coord - the coordinates
;;;;  marker - the marker
;;;; 
;;;; Returns:
;;;;  t if the marker was added, nil if the marker already existed.

(defmethod add-marker ((coords coordinates) marker &aux markedCoords (returnValue nil))

  ;; mark the given coordinates
  (setf markedCoords (gethash marker *marked_coordinates*))
  (if (null (find coords markedCoords :test #'equalp))
      (progn
	(setf (gethash marker *marked_coordinates*)
	      (append markedCoords (list coords)))
	(setf returnValue t)))
  ;; return the returnValue
  returnValue)



;;;; Function: remove-marker
;;;;
;;;; Removes a marker on the given coordinates.
;;;;
;;;; Arguments:
;;;;  coords - the coordinates
;;;;  marker - the marker to remove
;;;; 

(defmethod remove-marker ((coords coordinates) marker &aux markedCoords)
  
  ;; removes the marker from the given coordinates
  (setf markedCoords (gethash marker *marked_coordinates*))
  (setf (gethash marker *marked_coordinates*)
	(remove coords markedCoords :test #'equalp))
  t)





;;;;===========================================================================
;;;;
;;;; Perception utility functions
;;;;
;;;;===========================================================================


;;;; Function:  list-perceived-objects
;;;;
;;;; Gets a list of the objects in a perception.
;;;;
;;;; Arguments:
;;;;  percept - the perception
;;;;
;;;; Returns:
;;;;  List of all objects contained in the perception.

(defmethod list-perceived-objects ((percept perceptions) &aux objs)

  ;; collect all objects in the long-range sensors
  (setf objs (remove-if #'null
			(loop for x in (perceptions-longrange_sensors percept) 
			      collect (cell-occupant x))))
  ;; replace the long-range perceived obj with the short-range one
  (if (perceptions-shortrange_sensors percept)
      (let* ((obj_shortrange (perceptions-shortrange_sensors percept))
	     (coord (coordinates obj_shortrange)))
	(setf objs (remove-if #'(lambda(x) (equalp coord (coordinates x)))
			      objs))
	(setf objs (append objs (list obj_shortrange)))))
  objs)



;;;; Function:  locate-perceived-objects
;;;;
;;;; Gets a hashmap from a location to the perceived object
;;;; at that location.
;;;;
;;;; Arguments:
;;;;  percept - the perception
;;;;
;;;; Returns:
;;;;  A hashmap from a coordinate location to the perceived object
;;;;  at that location.

(defmethod locate-perceived-objects ((percept perceptions) &aux (locationToObjs (make-hash-table :test #'equalp)))

  ;; hash each perceived object by its location
  (loop for obj in (list-perceived-objects percept) do
	(setf (gethash (coordinates obj) locationToObjs) obj))
  ;; return the hash-table
  locationToObjs)



;;;; Function:  list-perceived-coordinates
;;;;
;;;; Gets a list of the coordinates in a perception.
;;;;
;;;; Arguments:
;;;;  percept - the perception
;;;;
;;;; Returns:
;;;;  List of all coordinates contained in the perception.

(defmethod list-perceived-coordinates ((percept perceptions))

  ;; collect all coordinates in the long-range sensors
  (loop for x in (perceptions-longrange_sensors percept) 
	collect (coordinates x)))


;;;; Function:  get-direction
;;;;
;;;; Finds the direction between two adjacent coordinates.
;;;;
;;;; Arguments:
;;;;  c1, c2 - Coordinate structures
;;;;
;;;; Returns:
;;;;  The direction (e.g. "NORTH", "SOUTH", "EAST", or "WEST")
;;;;  from c1 to c2, or NIL if the coordinates were not adjacent.

(defmethod get-direction ((c1 coordinates) (c2 coordinates))
    (if (adjacent-coordinates-p c1 c2)
        (cond
            ;; This assumes only the four cardinal directions
            ;; are possible, i.e. if the x coordinates don't match,
            ;; then the y coordinates must match (and we don't
            ;; need to compare them).

            ((< (x-coordinate c2) (x-coordinate c1))
                "WEST")
            ((> (x-coordinate c2) (x-coordinate c1))
                "EAST")
            ((> (y-coordinate c2) (y-coordinate c1))
                "NORTH")
            ((< (y-coordinate c2) (y-coordinate c1))
                "SOUTH")
            (t nil))))


;;;; Function:  get-current-location
;;;;
;;;; Gets the agent's current location from a perception
;;;;
;;;; Arguments:
;;;;  percept - the perception
;;;;
;;;; Returns:
;;;;  The agent's current location, as a coordinates structure.

(defmethod get-current-location ((percept perceptions))
    (coordinates (perceptions-self_sensors percept)))
